/*
 * Servo.c
 *
 * Created: 3/21/2013 6:50:14 AM
 *  Author: Parag
 */ 

#include "Servo.h"
/*
void ConfigRightServo(enum Angles ang)
{
	switch(ang)
		{
			case North:
					OCR1A = 310;
				break;
			case NW:
					OCR1A = 255;
				break;
			case WNW:
					OCR1A = 228;
				break;
			case West:
					OCR1A = 200;
				break;
			case WSW:
					OCR1A = 173;
				break;
			case SW:
					OCR1A = 145;
				break;
			case South:
					OCR1A = 90;
				break;
			default:
				OCR1A = 200;		
		}
}
void ConfigLeftServo(enum Angles ang)
{
	switch(ang)
	{
		case North:
				OCR1B = 90;
			break;
		case NW:
				OCR1B = 145;
			break;
		case WNW:
				OCR1B = 173;
			break;
		case West:
				OCR1B = 200;
			break;
		case WSW:
				OCR1B = 228;
			break;
		case SW:
				OCR1B = 255;
			break;
		case South:
				OCR1B = 310;
			break;
		default:
				OCR1B = 200;		
	}
}

*/
// set like left Servo orientation
void ConfigRudderServo(enum Angles ang)
{
	switch(ang)
	{
		case North:
			OCR1B = 90;
			break;
		case NW:
			OCR1B = 145;
			break;
		case WNW:
			OCR1B = 173;
			break;
		case West:
			OCR1B = 200;
			break;
		case WSW:
			OCR1B = 228;
			break;
		case SW:
			OCR1B = 255;
			break;
		case South:
			OCR1B = 310;
			break;
		default:
			OCR1B = 200;
	}
}

void ServoInit()
{
	//Configure TIMER1
	TCCR1A|=(1<<COM1A1)|(1<<COM1B1)|(1<<WGM11);			//NON Inverted PWM
	TCCR1B|=(1<<WGM13)|(1<<WGM12) |(1<<CS11)|(1<<CS10);	//PRESCALER=64 MODE 14(FAST PWM)

	ICR1=2499;											//Setting Top; fPWM=50Hz (Period = 20ms Standard)
	DDRB |= (1<<P_Rudder);									//PB6 = RudderServo															
	sei();
	
	ConfigRudderServo(West);
	_delay_ms(1000);									// allow the servos to initialize
}